#include "foc.h"

int sin_val_q15 ;
int cos_val_q15 ;
float sin_val_f32 ;
float cos_val_f32 ;
	
void inverse_clarke_q15(int64_t alpha, int64_t beta, int64_t *a, int64_t *b, int64_t *c)
{
	int32_t tempa;
	int64_t tempb;
	tempa = (alpha>>1);
	tempb = ((int64_t)28378*(int64_t)beta)>>15;
	*a = alpha;
	*b = (-tempa + tempb); // 0.8660254 = sqrt(3)/2
	*c = (-tempa - tempb);
//	*a = Q15_TO(*a);
//	*b = Q15_TO(*b);
//	*c = Q15_TO(*c);
}

void inverse_clarke_f32(float alpha, float beta, float *a, float *b, float *c)
{
	*a = alpha;
	*b = (-0.5*alpha + 0.8660254*beta); // 0.8660254 = sqrt(3)/2
	*c = (-0.5*alpha - 0.8660254*beta);
}

float triangle_waves(float x)
{
	x = fmod(0.159154943092 * x + 2.25,0.3333333);
	if(0 <= x && x < 0.1666666)
	{
		return 3.462921 * x - 0.28857675;
	}
	return 3.462921 * (0.3333333 - x) - 0.28857675;
}

int64_t triangle_waves_q15(int64_t x)
{
	x = (x + 73728) % 10923;
	if(0 <= x && x < 5461)
	{
		return ((113473 * x) >> 15) - 9456;
	}
	return ((113473 * (10923 - x)) >> 15) - 9456;
}

void svpwm_q15(int32_t d, int32_t q, int32_t theta, int32_t *a, int32_t *b, int32_t *c)
{
	sin_val_q15 = arm_sin_q15(theta);
	cos_val_q15 = arm_cos_q15(theta);
	int64_t alpha = (d * cos_val_q15 - q * sin_val_q15);
	int64_t beta  = (d * sin_val_q15 + q * cos_val_q15);
	int64_t triangle = q * triangle_waves_q15(theta);
	int64_t tempa = 0;
	int64_t tempb = 0;
	int64_t tempc = 0;
	inverse_clarke_q15(alpha,beta,&tempa,&tempb,&tempc);
	tempa = ((triangle + ((37824 * tempa) >> 15))) >> 15;
	tempb = ((triangle + ((37824 * tempb) >> 15))) >> 15;
	tempc = ((triangle + ((37824 * tempc) >> 15))) >> 15;
	*a = tempa;
	*b = tempb;
	*c = tempc;
}

void inverse_park_q15(int32_t d, int32_t q, int32_t theta, int32_t *alpha, int32_t *beta)
{
	sin_val_q15 = arm_sin_q15(theta);
	cos_val_q15 = arm_cos_q15(theta);
	*alpha = (d * cos_val_q15 - q * sin_val_q15);
	*beta = (d * sin_val_q15 + q * cos_val_q15);
}

void inverse_park_f32(float d, float q, float theta, float *alpha, float *beta)
{
	sin_val_f32 = arm_sin_f32(theta);
	cos_val_f32 = arm_cos_f32(theta);
	*alpha = (d * cos_val_f32 - q * sin_val_f32);
	*beta = (d * sin_val_f32 + q * cos_val_f32);
}

//int foc_findresidue_e_q15(foc_t *foc, int angle) {
//	for(int i = 0;i < foc->len - 1;i++)
//	{
//		if(foc->residue_e_q15[0][0] > angle)
//		{
//			return 0;
//		}
//		if(foc->residue_e_q15[foc->len][0] < angle)
//		{
//			return foc->len;
//		}
//		if(foc->residue_e_q15[i][0] == angle)
//		{
//			return i;
//		}
//		if(direction == 1)
//		{
//			if(foc->residue_e_q15[i][0] < angle && angle < foc->residue_e_q15[i + 1][0])
//			{
//				return i;
//			}
//		}
//	}
//	if(foc->residue_e_q15[foc->len][0] == angle)
//	{
//		return foc->len;
//	}
//	return foc->len;
//}

int32_t machinery_angle_to_electric_angle_q15(foc_t *foc, int32_t angle)
{
	foc->angle_q15 = (angle * foc->k_q15) + (foc->b_q15);// + foc->residue_e_q15[foc_findresidue_e_q15(foc,angle)][1];
	foc->angle_q15 %= 32768;
	return foc->angle_q15;
}

float machinery_angle_to_electric_angle_f32(foc_t *foc, int32_t angle)
{
	foc->angle_f32 = fmod((angle * foc->k_f32 ) + (foc->b_f32),FOC_2PI);
	return foc->angle_f32;
}

void foc_set_sector(foc_t *foc, uint8_t sector)
{
	switch(sector)
	{
		case 0:
			foc->set_va_pwm(foc->initv);
			foc->set_vb_pwm(0);
			foc->set_vc_pwm(0);
			break;
		case 1:
			foc->set_va_pwm(foc->initv);
			foc->set_vb_pwm(foc->initv);
			foc->set_vc_pwm(0);
			break;
		case 2:
			foc->set_va_pwm(0);
			foc->set_vb_pwm(foc->initv);
			foc->set_vc_pwm(0);
			break;
		case 3:
			foc->set_va_pwm(0);
			foc->set_vb_pwm(foc->initv);
			foc->set_vc_pwm(foc->initv);
			break;
		case 4:
			foc->set_va_pwm(0);
			foc->set_vb_pwm(0);
			foc->set_vc_pwm(foc->initv);
			break;
		case 5:
			foc->set_va_pwm(foc->initv);
			foc->set_vb_pwm(0);
			foc->set_vc_pwm(foc->initv);
			break;
	}
}

int foc_residue_e_q15_cmp(const void *a, const void *b) {
    const int (*rowA)[2] = (const int (*)[2])a;
    const int (*rowB)[2] = (const int (*)[2])b;
    return (*rowA)[0] - (*rowB)[0];
}

void foc_calibration(foc_t *foc, int *angle, int mid_angle)
{
	// 校准
	float temp_angle_x[128] = {0};
	float temp_angle_y[128] = {0};
	foc_set_sector(foc,0);
	foc->delay(foc->delay_x);
	float last_angle = *angle;
	float now_angle = *angle;
	uint16_t idx = 0;
	int8_t flag = 0;
	while (1)
	{
		foc_set_sector(foc, idx % 6);
		foc->delay(foc->delay_x);
		last_angle = now_angle;
		now_angle = *angle;
		float tempe = now_angle - last_angle;
		if (tempe > mid_angle)
		{
			flag = -1;
		}
		else if (tempe < -mid_angle)
		{
			flag = 1;
		}
		temp_angle_x[idx] = *angle + ((mid_angle << 1) * flag);
		//foc->residue_e_q15[idx][0] = *angle;
		temp_angle_y[idx] = idx * 1.047197551195;
		if (idx >= 500)
			break;
		if (fabs(temp_angle_x[idx] - temp_angle_x[0]) > 4096)
			break;
		idx++;
	}
	
	foc->len = idx;

	float sum_xy = 0, sum_x = 0, sum_y = 0, sum_pow_x = 0, pow_sum_x = 0;
	for (int i = 0; i < idx; i++)
	{
		sum_xy += temp_angle_x[i] * temp_angle_y[i];
		sum_x += temp_angle_x[i];
		sum_y += temp_angle_y[i];
		pow_sum_x += temp_angle_x[i] * temp_angle_x[i];
	}
	sum_pow_x = sum_x * sum_x;
	foc->k_f32 = (idx * sum_xy - sum_x * sum_y) / (idx * pow_sum_x - sum_pow_x);
	foc->b_f32 = (sum_y - foc->k_f32 * sum_x) / idx;
	
	foc->k_q15 = (float)foc->k_f32 * 5215.18917524;
	foc->b_q15 = (float)foc->b_f32 * 5215.18917524;
	
//	for (int i = 0; i < idx; i++)
//	{
//		float residue_e = temp_angle_y[i] - (foc->k_f32 * temp_angle_x[i] + foc->b_f32);
//		foc->residue_e_q15[i][1] = residue_e * 5215.18917524;
//	}
//	
//	qsort(foc->residue_e_q15, idx + 1, sizeof(foc->residue_e_q15[0]), foc_residue_e_q15_cmp);
	
	if(foc->k_f32 >= 0)
	{
		foc->direction = 1;
	}
	else
	{
		foc->direction = -1;
	}
	
	foc->ready = 1;
}

int32_t zero_processing(int32_t target, int32_t fb, int32_t midfb)
{
	int32_t e = target - fb;
	if (e > midfb)
	{
		return fb + (midfb << 1);
	}
	else if (e < -midfb)
	{
		return fb - (midfb << 1);
	}
	return fb;
}
